#include "Gimbal.h"

float FCC[3]={0};
gimbal_t Gimbal;
imu_t imu;
/* 云台电机期望角度(degree) */
float yaw_angle_ref;
float pitch_angle_ref;
/* 云台电机电流 */
int16_t yaw_moto_current;
int16_t pitch_moto_current;
volatile float yaw_relative_angle;
volatile float pitch_relative_angle;
gimbal_back_e gimbal_back_step;
float yaw_angle_fdb = 0;
float pitch_angle_fdb = 0;
float angle_measure;
float speed = 0;
float angle = 50;
float Pitch_Speed = 0;
int Compensation_Current = 0;
float pit_angle = 0;



void Gimbal_Task(void)
{

    /*获取云台传感器信息*/
    Gimbal_Get_Information();
    /*设置云台运动模式*/
    Gimbal_Set_Mode();
    /*云台控制*/
    Gimbal_Control();
    /*云台电机电流计算*/
    Gimbal_Calculate();
    Gimbal.last_mode=Gimbal.ctrl_mode;
}

void Gimbal_Init_Para(void)
{
    /*云台Pitch轴电机PID初始化*/
    PID_Init(&PitchMotor.PID_Velocity,PITCH_V_PID_MAXOUT,PITCH_V_PID_MAXINTEGRAL,PITCH_V_PID_DEADBAND,
             PITCH_V_PID_KP,PITCH_V_PID_KI,PITCH_V_PID_KD,PITCH_V_PID_A,PITCH_V_PID_B,PITCH_V_PID_O_LPF,
             PITCH_V_PID_D_LPF,ols_order,Integral_Limit | Trapezoid_Intergral | OutputFilter | DerivativeFilter);
    FCC[0] = PITCH_V_FCC_C0;
    FCC[1] = PITCH_V_FCC_C1;
    FCC[2] = PITCH_V_FCC_C2;
    Feedforward_Init(&PitchMotor.FFC_Velocity,PITCH_V_FFC_MAXOUT,FCC,PITCH_V_FCC_LPF,4,4);
    PID_Init(&PitchMotor.PID_Angle,PITCH_A_PID_MAXOUT,PITCH_A_PID_MAXINTEGRAL,PITCH_A_PID_DEADBAND,
             PITCH_A_PID_KP,PITCH_A_PID_KI,PITCH_A_PID_KD,PITCH_A_PID_A,PITCH_A_PID_B,PITCH_A_PID_O_LPF,
             PITCH_A_PID_D_LPF,ols_order,Integral_Limit | Trapezoid_Intergral | OutputFilter | DerivativeFilter);
    FCC[0] = PITCH_A_FCC_C0;
    FCC[1] = PITCH_A_FCC_C1;
    FCC[2] = PITCH_A_FCC_C2;
    Feedforward_Init(&PitchMotor.FFC_Angle,PITCH_A_FFC_MAXOUT,FCC,PITCH_A_FCC_LPF,4,4);

    /*云台Yaw轴电机PID初始化*/
    PID_Init(&YawMotor.PID_Velocity,YAW_V_PID_MAXOUT,YAW_V_PID_MAXINTEGRAL,YAW_V_PID_DEADBAND,YAW_V_PID_KP,
             YAW_V_PID_KI,YAW_V_PID_KD,YAW_V_PID_A,YAW_V_PID_B,YAW_V_PID_O_LPF,YAW_V_PID_D_LPF,ols_order,
             Integral_Limit | Trapezoid_Intergral | OutputFilter | DerivativeFilter);

    PID_Init(&YawMotor.PID_Angle,YAW_A_PID_MAXOUT,YAW_A_PID_MAXINTEGRAL,YAW_A_PID_DEADBAND,YAW_A_PID_KP,
             YAW_A_PID_KI,YAW_A_PID_KD,YAW_A_PID_A,YAW_A_PID_B,YAW_A_PID_O_LPF,YAW_A_PID_D_LPF,ols_order,
             Integral_Limit | Trapezoid_Intergral | OutputFilter | DerivativeFilter);

    /*云台初始校准，设置云台中点值*/
    calibrated();
    Gimbal.ctrl_mode=GIMBAL_INIT;
}

void Gimbal_Get_Information(void) {
    /*获取姿态解算后的数据*/
    get_imu_data(&imu);

    /*获取云台的相对角度*/
    if(Gimbal_cail.calied_flag = 1)
    {
        yaw_relative_angle = get_relative_pos(YawMotor.RawAngle, Gimbal_cail.yaw_offset) / 22.75f;
        pitch_relative_angle = get_relative_pos(PitchMotor.RawAngle, Gimbal_cail.pit_offset) / 22.75f;
    }
    else
        calibrated();
    send_chassis_moto_zero_current();

}

static int16_t get_relative_pos(int16_t raw_ecd, int16_t center_offset)
{
    int16_t tmp = 0;
    if (center_offset >= 4096)
    {
        if (raw_ecd > center_offset - 4096)
            tmp = raw_ecd - center_offset;
        else
            tmp = raw_ecd + 8192 - center_offset;
    }
    else
    {
        if (raw_ecd > center_offset + 4096)
            tmp = raw_ecd - 8192 - center_offset;
        else
            tmp = raw_ecd - center_offset;
    }
    return tmp;
}

void Gimbal_Set_Mode(void)
{
    if (rc.sw1 == RC_UP)
    {
        Gimbal.ctrl_mode = GIMBAL_INIT;
        gimbal_back_param();
        Gimbal.ac_mode=NO_ACTION;
    }
    else if (rc.sw1 == RC_MI)
    {
        Gimbal.ctrl_mode = GIMBAL_LOCK;
        Gimbal.ac_mode=NO_ACTION;
    }
    else if(rc.sw1 == GIMBAL_REMOTE)
    {
        Gimbal.ctrl_mode = GIMBAL_REMOTE;
        Gimbal.ac_mode=IS_ACTION;
    }
}



void Gimbal_Control(void)
{
    if(Gimbal.ctrl_mode==GIMBAL_INIT)
        Gimbal_Init_Mi();
    else if (Gimbal.ctrl_mode==GIMBAL_REMOTE)
        Gimbal_Control_Remote();
}

void Gimbal_Init_Mi(void)
{
    pitch_angle_fdb = pitch_relative_angle;
    yaw_angle_fdb = yaw_relative_angle;

    /* gimbal pitch back center */
    pitch_angle_ref = pitch_relative_angle * (1 - ramp_calc(&pitch_ramp));

    switch (gimbal_back_step)
    {
        case BACK_PIT_STEP:
        {
            /* keep yaw unmove this time */
            yaw_angle_ref = Gimbal.ecd_offset_angle;
            if(fabs(pitch_angle_fdb) <= 2.0f)
                gimbal_back_step = YAW_BACK_STEP;
        }break;

        case YAW_BACK_STEP:
        {
            yaw_angle_ref = yaw_relative_angle * ( 1 - ramp_calc(&yaw_ramp));
            if (fabs(yaw_angle_fdb) <= 2.0f)
                gimbal_back_step = BACK_IS_OK;
        }break;

        case BACK_IS_OK:
        {
            Gimbal.ctrl_mode = GIMBAL_REMOTE;
            Gimbal.yaw_offset_angle = imu.angle_x;
            Gimbal.pit_offset_angle = imu.angle_y;
            pitch_angle_ref = 0;
            yaw_angle_ref = 0;
        }break;

    }
}

void Gimbal_Calculate(void)
{
    /* pitch轴俯仰角度限制 */
    VAL_LIMIT(pitch_angle_ref, PITCH_ANGLE_MIN, PITCH_ANGLE_MAX);
    /* yaw轴俯仰角度限制 */
    VAL_LIMIT(yaw_angle_ref, YAW_ANGLE_MIN, YAW_ANGLE_MAX);
    /*pitch轴速度为gyro_x*/
    Pitch_Speed = -PID_Calculate(&PitchMotor.PID_Angle,rel_angle(PitchMotor.RawAngle),pitch_angle_ref);
    pitch_moto_current  =  Gravity_Compensation(PitchMotor.RawAngle) - PID_Calculate(&PitchMotor.PID_Velocity,-PitchMotor.Velocity_RPM,Pitch_Speed);
    //pitch_moto_current=Motor_Angle_Calculate(&PitchMotor, pitch_relative_angle,imu.gyro_x,pitch_angle_ref);
    /*yaw轴速度为gyro_z*/
    angle_measure = YawMotor.total_angle*360.0/8192;
    speed = PID_Calculate(&YawMotor.PID_Angle, angle_measure, yaw_angle_ref);                                                    //
    yaw_moto_current = PID_Calculate(&YawMotor.PID_Velocity,YawMotor.Velocity_RPM,speed);
    if (pitch_moto_current>PitchMotor.PID_Velocity.MaxOut)
    {
        pitch_moto_current = PitchMotor.PID_Velocity.MaxOut;
    }
    else if (pitch_moto_current<-PitchMotor.PID_Velocity.MaxOut)
    {
        pitch_moto_current = - PitchMotor.PID_Velocity.MaxOut;
    }
    //yaw_moto_current=Motor_Angle_Calculate(&YawMotor, yaw_relative_angle,imu.gyro_z,yaw_angle_ref);

}

static float rel_angle(int encoder_angle)
{
    if (encoder_angle<6000)
    {
        pit_angle = (encoder_angle+8192-6800)/22.75;
    }
    else
    {
        pit_angle = (encoder_angle-6800)/22.75;
    }
    return pit_angle;
}


static int Gravity_Compensation(int encoder_angle )
{

    if (encoder_angle<6000)
    {
        pit_angle = (encoder_angle+8192-6800)/22.75;
    }
    else
    {
        pit_angle = (encoder_angle-6800)/22.75;
    }
    Compensation_Current = 0.6298*pit_angle*pit_angle+26.904*pit_angle-9378.7;
    return Compensation_Current;
}

static void gimbal_back_param(void)
{
    gimbal_back_step = BACK_PIT_STEP;
    Gimbal.ecd_offset_angle = yaw_relative_angle;

    ramp_init(&pitch_ramp, BACK_CENTER_TIME/GIMBAL_PERIOD);
    ramp_init(&yaw_ramp, BACK_CENTER_TIME/GIMBAL_PERIOD);
}

void Gimbal_Control_Remote(void)
{
    pitch_angle_fdb = imu.angle_y - Gimbal.pit_offset_angle;//pit轴当前与初始角度差
    yaw_angle_fdb = imu.angle_x - Gimbal.yaw_offset_angle;//yaw轴当前与初始角度差;
    if(Gimbal.ctrl_mode==GIMBAL_REMOTE)
    {
        /*yaw轴的角度累加，单位degree*/
        yaw_angle_ref += rc.ch3 * RC_RATIO * GIMBAL_RC_MOVE_RATIO_YAW;

        /*pitch轴的角度累加，单位degree*/
        pitch_angle_ref += rc.ch4 * RC_RATIO * GIMBAL_RC_MOVE_RATIO_PIT;
    }

    /*限制pitch轴的活动角度*/
    if ((pitch_relative_angle >= PITCH_ANGLE_MIN) && (pitch_relative_angle <= PITCH_ANGLE_MAX))
    {
        VAL_LIMIT(pitch_angle_ref, PITCH_ANGLE_MIN, PITCH_ANGLE_MAX);
    }

    /*限制yaw轴的活动角度*/
    if ((yaw_relative_angle >= YAW_ANGLE_MIN) && (yaw_relative_angle <= YAW_ANGLE_MAX))
    {
        VAL_LIMIT(yaw_angle_ref, YAW_ANGLE_MIN, YAW_ANGLE_MAX);
    }
}